#include <GenericTypeDefs.h>
#include <string.h>

#include "Protocol.h"
#include "LED.h"

#define PROTOCOL_HEADER "SET_LEDS"
#define PROTOCOL_REQUEST_PARAM_TYPE PARAM_BINARY
#define PROTOCOL_REQUEST_PARAM_LENGTH 3

static int SetLedsProtocol_matchesRequest(const BYTE* request, WORD requestLength);
static void SetLedsProtocol_buildResponse(const BYTE* request, WORD requestLength, BYTE* response, WORD* responseLength);

Protocol SetLedsProtocol = {
	SetLedsProtocol_matchesRequest,
	SetLedsProtocol_buildResponse
};

static int SetLedsProtocol_matchesRequest(const BYTE* request, WORD requestLength) {
	int cursor = 0;

	if (memcmp(request, PROTOCOL_HEADER PROTOCOL_PARAM_BEGIN, sizeof(PROTOCOL_HEADER PROTOCOL_PARAM_BEGIN) - 1) != 0)
		return FALSE;
	cursor += sizeof(PROTOCOL_HEADER PROTOCOL_PARAM_BEGIN) - 1;

	if (!isArgsCompatible(request + cursor, PROTOCOL_REQUEST_PARAM_TYPE, PROTOCOL_REQUEST_PARAM_LENGTH))
		return FALSE;
	cursor += PROTOCOL_REQUEST_PARAM_LENGTH;

	if (memcmp(request + cursor, PROTOCOL_PARAM_END PROTOCOL_FOOTER, sizeof(PROTOCOL_PARAM_END PROTOCOL_FOOTER) -1) != 0)
		return FALSE;

	return TRUE;
}

static void SetLedsProtocol_buildResponse(const BYTE* request, WORD requestLength, BYTE* response, WORD* responseLength) {
	LedFlags ledFlags = LED_NONE;
	const char* params;

	params = strstr((char*)request, PROTOCOL_PARAM_BEGIN) + 1;

	ledFlags |= (*params++ == '1' ? LED_1 : LED_NONE);
	ledFlags |= (*params++ == '1' ? LED_2 : LED_NONE);
	ledFlags |= (*params++ == '1' ? LED_3 : LED_NONE);
	
	Leds.set(ledFlags);

	memcpy(response, PROTOCOL_HEADER PROTOCOL_FOOTER, sizeof(PROTOCOL_HEADER PROTOCOL_FOOTER) - 1);
	*responseLength = sizeof(PROTOCOL_HEADER PROTOCOL_FOOTER) - 1;
}
